﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using TrackingSim.Filtering;
using Microsoft.Xna.Framework;
using TrackingSim.Filtering.Measurements;

namespace TrackingSim.GameSystem
{
    class Radar
    {
        const double range_random_std = 1.0;
        const double az_random_std = 0.05;
        const double el_random_std = 0.05;
        static Random r = new Random();
        Player host;
        Dictionary<int,List<Rcycz>> measurements_rcycz;
        Dictionary<int,List<Razel>> measurements_razel;

        public Radar(Player _host)
        {
            host = _host;
            measurements_rcycz = new Dictionary<int, List<Rcycz>>();
            measurements_razel = new Dictionary<int, List<Razel>>();
        }

        public Razel measure_razel(Player p, double time)
        {
            Razel meas = new Razel();
            meas.Time = time;

            meas.TruePos.v[0] = p.Pos.X;
            meas.TruePos.v[1] = p.Pos.Y;
            meas.TruePos.v[2] = p.Pos.Z;

            Vector3 delta_pos = p.Pos - host.Pos;

            MathLib.Vector delta_pos_abs = Coords.get_vector_from_xna_vector(delta_pos);
            MathLib.Vector delta_pos_body = Coords.abs_to_body(host) * delta_pos_abs;

            Vector3 transformed_delta = Coords.get_xna_vector_from_vector(delta_pos_body);

            meas.R = noised_meas(delta_pos.Length(), range_random_std);
            meas.R_random_std = range_random_std;
            meas.R_bias_std = 0.0;

            meas.TrueR = delta_pos.Length();

            System.Console.WriteLine("X: " + transformed_delta.X + " , Y: " + transformed_delta.Y + " , Z: " + transformed_delta.Z);

            transformed_delta.Normalize();
            meas.Az = Math.Atan2(transformed_delta.Y, transformed_delta.X);
            double horiz_mag = Math.Sqrt(transformed_delta.X * transformed_delta.X + transformed_delta.Y * transformed_delta.Y);
            meas.El = Math.Atan2(transformed_delta.Z , horiz_mag);

            meas.Az_random_std = az_random_std;
            meas.El_random_std = el_random_std;

            double az_deg = meas.Az * 180.0 / Math.PI;
            double el_deg = meas.El * 180.0 / Math.PI;

            meas.UnitLOS.v[0] = transformed_delta.X;
            meas.UnitLOS.v[1] = transformed_delta.Y;
            meas.UnitLOS.v[2] = transformed_delta.Z;

            if (!measurements_razel.ContainsKey(p.PlayerNumber))
            {
                measurements_razel.Add(p.PlayerNumber, new List<Razel>());
            }

            measurements_razel[p.PlayerNumber].Add(meas);

            return meas;
        }

        public Rcycz measure_rcycz(Player p, double time)
        {
            Rcycz meas = new Rcycz();
            meas.Time = time;

            meas.TruePos.v[0] = p.Pos.X;
            meas.TruePos.v[1] = p.Pos.Y;
            meas.TruePos.v[2] = p.Pos.Z;

            Vector3 delta_pos = p.Pos - host.Pos;

            Matrix abs_to_body = Coords.get_xna_matrix_from_rotation_matrix(Coords.abs_to_body(host));

            Vector3 transformed_delta = Vector3.Transform(delta_pos, abs_to_body);

            meas.R = noised_meas(delta_pos.Length(), range_random_std);
            meas.R_random_std = range_random_std;
            meas.R_bias_std = 0.0;

            meas.TrueR = delta_pos.Length();

            System.Console.WriteLine("X: " + transformed_delta.X + " , Y: " + transformed_delta.Y + " , Z: " + transformed_delta.Z);

            transformed_delta.Normalize();
            meas.Cy = transformed_delta.Y;
            meas.Cz = transformed_delta.Z;

            meas.UnitLOS.v[0] = transformed_delta.X;
            meas.UnitLOS.v[1] = transformed_delta.Y;
            meas.UnitLOS.v[2] = transformed_delta.Z;

            if (!measurements_rcycz.ContainsKey(p.PlayerNumber))
            {
                measurements_rcycz.Add(p.PlayerNumber, new List<Rcycz>()); 
            }

            measurements_rcycz[p.PlayerNumber].Add(meas);

            return meas;
        }

        private static double noised_meas(double meas, double std)
        {
            double u1 = r.NextDouble();
            double u2 = r.NextDouble();

            double std_norm = Math.Sqrt(-2.0 * Math.Log(u1)) * Math.Cos(2.0 * Math.PI * u2);

            double noised = meas + std_norm * std;

            return noised;
        }
    }
}
